/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "precomp.hpp"

#define MAX_FV_SIZE 5
#define BLOB_NUM    5

typedef struct DefBlobFVN {
	CvBlob  blob;
	CvBlob  BlobSeq[BLOB_NUM];
	int     state;
	int     LastFrame;
	int     FrameNum;
} DefBlobFVN;

class CvBlobTrackFVGenN: public CvBlobTrackFVGen {
private:
	CvBlobSeq       m_BlobList;
	CvMemStorage*   m_pMem;
	CvSeq*          m_pFVSeq;
	float           m_FVMax[MAX_FV_SIZE];
	float           m_FVMin[MAX_FV_SIZE];
	float           m_FVVar[MAX_FV_SIZE];
	int             m_Dim;
	CvBlob          m_BlobSeq[BLOB_NUM];
	int             m_Frame;
	int             m_State;
	int             m_LastFrame;
	int             m_ClearFlag;
	void Clear() {
		if (m_pMem) {
			cvClearMemStorage(m_pMem);
			m_pFVSeq = cvCreateSeq(0, sizeof(CvSeq), sizeof(float) * (m_Dim + 1), m_pMem);
			m_ClearFlag = 1;
		}
	}
public:
	CvBlobTrackFVGenN(int dim = 2 ): m_BlobList(sizeof(DefBlobFVN)) {
		int i;
		assert(dim <= MAX_FV_SIZE);
		m_Dim = dim;
		for (i = 0; i < m_Dim; ++i) {
			m_FVVar[i] = 0.01f;
			m_FVMax[i] = 1;
			m_FVMin[i] = 0;
		}
		m_Frame = 0;
		m_State = 0;
		m_pMem = cvCreateMemStorage();
		m_pFVSeq = NULL;
		Clear();

		switch (dim) {
		case 2: SetModuleName("P"); break;
		case 4: SetModuleName("PV"); break;
		case 5: SetModuleName("PVS"); break;
		}
	};

	~CvBlobTrackFVGenN() {
		if (m_pMem) { cvReleaseMemStorage(&m_pMem); }
	};

	void AddBlob(CvBlob* pBlob) {
		float       FV[MAX_FV_SIZE+1];
		int         i;
		DefBlobFVN* pFVBlob = (DefBlobFVN*)m_BlobList.GetBlobByID(CV_BLOB_ID(pBlob));

		if (!m_ClearFlag) { Clear(); }

		if (pFVBlob == NULL) {
			DefBlobFVN BlobNew;
			BlobNew.blob = pBlob[0];
			BlobNew.LastFrame = m_Frame;
			BlobNew.state = 0;;
			BlobNew.FrameNum = 0;
			m_BlobList.AddBlob((CvBlob*)&BlobNew);
			pFVBlob = (DefBlobFVN*)m_BlobList.GetBlobByID(CV_BLOB_ID(pBlob));
		} /* Add new record if necessary. */

		pFVBlob->blob = pBlob[0];

		/* Shift: */
		for (i = (BLOB_NUM - 1); i > 0; --i) {
			pFVBlob->BlobSeq[i] = pFVBlob->BlobSeq[i-1];
		}

		pFVBlob->BlobSeq[0] = pBlob[0];

		if (m_Dim > 0) {
			/* Calculate FV position: */
			FV[0] = CV_BLOB_X(pBlob);
			FV[1] = CV_BLOB_Y(pBlob);
		}

		if (m_Dim <= 2) {
			/* Add new FV if position is enough: */
			*(int*)(FV + m_Dim) = CV_BLOB_ID(pBlob);
			cvSeqPush( m_pFVSeq, FV );
		} else if (pFVBlob->FrameNum > BLOB_NUM) {
			/* Calculate velocity for more complex FV: */
			float       AverVx = 0;
			float       AverVy = 0;
			{
				/* Average velocity: */
				CvBlob* pBlobSeq = pFVBlob->BlobSeq;
				int     i;
				for (i = 1; i < BLOB_NUM; ++i) {
					AverVx += CV_BLOB_X(pBlobSeq + i - 1) - CV_BLOB_X(pBlobSeq + i);
					AverVy += CV_BLOB_Y(pBlobSeq + i - 1) - CV_BLOB_Y(pBlobSeq + i);
				}
				AverVx /= BLOB_NUM - 1;
				AverVy /= BLOB_NUM - 1;

				FV[2] = AverVx;
				FV[3] = AverVy;
			}

			if (m_Dim > 4) {
				/* State duration: */
				float T = (CV_BLOB_WX(pBlob) + CV_BLOB_WY(pBlob)) * 0.01f;

				if ( fabs(AverVx) < T && fabs(AverVy) < T) {
					pFVBlob->state++;
				} else {
					pFVBlob->state = 0;
				}
				FV[4] = (float)pFVBlob->state;
			} /* State duration. */

			/* Add new FV: */
			*(int*)(FV + m_Dim) = CV_BLOB_ID(pBlob);
			cvSeqPush( m_pFVSeq, FV );

		} /* If velocity is calculated. */

		pFVBlob->FrameNum++;
		pFVBlob->LastFrame = m_Frame;
	};  /* AddBlob */

	void Process(IplImage* pImg, IplImage* /*pFG*/) {
		int i;
		if (!m_ClearFlag) { Clear(); }
		for (i = m_BlobList.GetBlobNum(); i > 0; --i) {
			/* Delete unused blob: */
			DefBlobFVN* pFVBlob = (DefBlobFVN*)m_BlobList.GetBlob(i - 1);
			if (pFVBlob->LastFrame < m_Frame) {
				m_BlobList.DelBlob(i - 1);
			}
		} /* Check next blob in list. */

		m_FVMin[0] = 0;
		m_FVMin[1] = 0;
		m_FVMax[0] = (float)(pImg->width - 1);
		m_FVMax[1] = (float)(pImg->height - 1);
		m_FVVar[0] = m_FVMax[0] * 0.01f;
		m_FVVar[1] = m_FVMax[1] * 0.01f;
		m_FVVar[2] = (float)(pImg->width - 1) / 1440.0f;
		m_FVMax[2] = (float)(pImg->width - 1) * 0.02f;
		m_FVMin[2] = -m_FVMax[2];
		m_FVVar[3] = (float)(pImg->width - 1) / 1440.0f;
		m_FVMax[3] = (float)(pImg->height - 1) * 0.02f;
		m_FVMin[3] = -m_FVMax[3];
		m_FVMax[4] = 25 * 32.0f; /* max state is 32 sec */
		m_FVMin[4] = 0;
		m_FVVar[4] = 10;

		m_Frame++;
		m_ClearFlag = 0;
	};
	virtual void    Release() {delete this;};
	virtual int     GetFVSize() {return m_Dim;};
	virtual int     GetFVNum() {
		return m_pFVSeq->total;
	};

	virtual float*  GetFV(int index, int* pFVID) {
		float* pFV = (float*)cvGetSeqElem( m_pFVSeq, index );
		if (pFVID) { pFVID[0] = *(int*)(pFV + m_Dim); }
		return pFV;
	};
	virtual float*  GetFVMin() {return m_FVMin;}; /* returned pointer to array of minimal values of FV, if return 0 then FVrange is not exist */
	virtual float*  GetFVMax() {return m_FVMax;}; /* returned pointer to array of maximal values of FV, if return 0 then FVrange is not exist */
	virtual float*  GetFVVar() {return m_FVVar;}; /* returned pointer to array of maximal values of FV, if return 0 then FVrange is not exist */
};/* CvBlobTrackFVGenN */

CvBlobTrackFVGen* cvCreateFVGenP() {return (CvBlobTrackFVGen*)new CvBlobTrackFVGenN(2);}
CvBlobTrackFVGen* cvCreateFVGenPV() {return (CvBlobTrackFVGen*)new CvBlobTrackFVGenN(4);}
CvBlobTrackFVGen* cvCreateFVGenPVS() {return (CvBlobTrackFVGen*)new CvBlobTrackFVGenN(5);}
#undef MAX_FV_SIZE

#define MAX_FV_SIZE 4
class CvBlobTrackFVGenSS: public CvBlobTrackFVGen {
private:
	CvBlobSeq       m_BlobList;
	CvMemStorage*   m_pMem;
	CvSeq*          m_pFVSeq;
	float           m_FVMax[MAX_FV_SIZE];
	float           m_FVMin[MAX_FV_SIZE];
	float           m_FVVar[MAX_FV_SIZE];
	int             m_Dim;
	CvBlob          m_BlobSeq[BLOB_NUM];
	int             m_Frame;
	int             m_State;
	int             m_LastFrame;
	int             m_ClearFlag;
	void Clear() {
		cvClearMemStorage(m_pMem);
		m_pFVSeq = cvCreateSeq(0, sizeof(CvSeq), sizeof(float) * (m_Dim + 1), m_pMem);
		m_ClearFlag = 1;
	}
public:
	CvBlobTrackFVGenSS(int dim = 2 ): m_BlobList(sizeof(DefBlobFVN)) {
		int i;
		assert(dim <= MAX_FV_SIZE);
		m_Dim = dim;
		for (i = 0; i < m_Dim; ++i) {
			m_FVVar[i] = 0.01f;
			m_FVMax[i] = 1;
			m_FVMin[i] = 0;
		}
		m_Frame = 0;
		m_State = 0;
		m_pMem = cvCreateMemStorage();
		m_pFVSeq = NULL;

		SetModuleName("SS");
	};
	~CvBlobTrackFVGenSS() {
		if (m_pMem) { cvReleaseMemStorage(&m_pMem); }
	};

	void AddBlob(CvBlob* pBlob) {
		//float       FV[MAX_FV_SIZE+1];
		int         i;
		DefBlobFVN* pFVBlob = (DefBlobFVN*)m_BlobList.GetBlobByID(CV_BLOB_ID(pBlob));

		if (!m_ClearFlag) { Clear(); }

		if (pFVBlob == NULL) {
			DefBlobFVN BlobNew;
			BlobNew.blob = pBlob[0];
			BlobNew.LastFrame = m_Frame;
			BlobNew.state = 0;;
			BlobNew.FrameNum = 0;
			m_BlobList.AddBlob((CvBlob*)&BlobNew);
			pFVBlob = (DefBlobFVN*)m_BlobList.GetBlobByID(CV_BLOB_ID(pBlob));
		} /* Add new record if necessary. */

		/* Shift: */
		for (i = (BLOB_NUM - 1); i > 0; --i) {
			pFVBlob->BlobSeq[i] = pFVBlob->BlobSeq[i-1];
		}

		pFVBlob->BlobSeq[0] = pBlob[0];

		if (pFVBlob->FrameNum > BLOB_NUM) {
			/* Average velocity: */
			CvBlob* pBlobSeq = pFVBlob->BlobSeq;
			float   T = (CV_BLOB_WX(pBlob) + CV_BLOB_WY(pBlob)) * 0.01f;
			float   AverVx = 0;
			float   AverVy = 0;
			int     i;
			for (i = 1; i < BLOB_NUM; ++i) {
				AverVx += CV_BLOB_X(pBlobSeq + i - 1) - CV_BLOB_X(pBlobSeq + i);
				AverVy += CV_BLOB_Y(pBlobSeq + i - 1) - CV_BLOB_Y(pBlobSeq + i);
			}
			AverVx /= BLOB_NUM - 1;
			AverVy /= BLOB_NUM - 1;

			if ( fabs(AverVx) < T && fabs(AverVy) < T) {
				pFVBlob->state++;
			} else {
				pFVBlob->state = 0;
			}
		}

		if (pFVBlob->state == 5) {
			/* Object is stopped:  */
			float   FV[MAX_FV_SIZE];
			FV[0] = pFVBlob->blob.x;
			FV[1] = pFVBlob->blob.y;
			FV[2] = pFVBlob->BlobSeq[0].x;
			FV[3] = pFVBlob->BlobSeq[0].y;
			*(int*)(FV + m_Dim) = CV_BLOB_ID(pBlob);
			cvSeqPush( m_pFVSeq, FV );
		} /* Object is stopped. */

		pFVBlob->FrameNum++;
		pFVBlob->LastFrame = m_Frame;
	};  /* AddBlob */
	void Process(IplImage* pImg, IplImage* /*pFG*/) {
		int i;

		if (!m_ClearFlag) { Clear(); }

		for (i = m_BlobList.GetBlobNum(); i > 0; --i) {
			/* Delete unused blob: */
			DefBlobFVN* pFVBlob = (DefBlobFVN*)m_BlobList.GetBlob(i - 1);
			if (pFVBlob->LastFrame < m_Frame) {
				float   FV[MAX_FV_SIZE+1];
				FV[0] = pFVBlob->blob.x;
				FV[1] = pFVBlob->blob.y;
				FV[2] = pFVBlob->BlobSeq[0].x;
				FV[3] = pFVBlob->BlobSeq[0].y;
				*(int*)(FV + m_Dim) = CV_BLOB_ID(pFVBlob);
				cvSeqPush( m_pFVSeq, FV );
				m_BlobList.DelBlob(i - 1);
			}
		} /* Check next blob in list. */

		/* Set max min range: */
		m_FVMin[0] = 0;
		m_FVMin[1] = 0;
		m_FVMin[2] = 0;
		m_FVMin[3] = 0;
		m_FVMax[0] = (float)(pImg->width - 1);
		m_FVMax[1] = (float)(pImg->height - 1);
		m_FVMax[2] = (float)(pImg->width - 1);
		m_FVMax[3] = (float)(pImg->height - 1);
		m_FVVar[0] = m_FVMax[0] * 0.01f;
		m_FVVar[1] = m_FVMax[1] * 0.01f;
		m_FVVar[2] = m_FVMax[2] * 0.01f;
		m_FVVar[3] = m_FVMax[3] * 0.01f;

		m_Frame++;
		m_ClearFlag = 0;
	};
	virtual void    Release() {delete this;};
	virtual int     GetFVSize() {return m_Dim;};
	virtual int     GetFVNum() {
		return m_pFVSeq->total;
	};

	virtual float*  GetFV(int index, int* pFVID) {
		float* pFV = (float*)cvGetSeqElem( m_pFVSeq, index );
		if (pFVID) { pFVID[0] = *(int*)(pFV + m_Dim); }
		return pFV;
	};

	virtual float*  GetFVMin() {return m_FVMin;}; /* returned pointer to array of minimal values of FV, if return 0 then FVrange is not exist */
	virtual float*  GetFVMax() {return m_FVMax;}; /* returned pointer to array of maximal values of FV, if return 0 then FVrange is not exist */
	virtual float*  GetFVVar() {return m_FVVar;}; /* returned pointer to array of maximal values of FV, if return 0 then FVrange is not exist */
};/* CvBlobTrackFVGenSS */

CvBlobTrackFVGen* cvCreateFVGenSS() {return (CvBlobTrackFVGen*)new CvBlobTrackFVGenSS;}

/*======================= TRAJECTORY ANALYZER MODULES =====================*/
/* Trajectory Analyser module */
#define SPARSE  0
#define ND      1
#define BYSIZE  -1
class DefMat {
private:
	CvSparseMatIterator m_SparseIterator;
	CvSparseNode*       m_pSparseNode;
	int*                m_IDXs;
	int                 m_Dim;

public:
	CvSparseMat*        m_pSparse;
	CvMatND*            m_pND;
	int                 m_Volume;
	int                 m_Max;
	DefMat(int dim = 0, int* sizes = NULL, int type = SPARSE) {
		/* Create sparse or ND matrix but not both: */
		m_pSparseNode = NULL;
		m_pSparse = NULL;
		m_pND = NULL;
		m_Volume = 0;
		m_Max = 0;
		m_IDXs = NULL;
		m_Dim = 0;
		if (dim > 0 && sizes != 0) {
			Realloc(dim, sizes, type);
		}
	}
	~DefMat() {
		if (m_pSparse) { cvReleaseSparseMat(&m_pSparse); }
		if (m_pND) { cvReleaseMatND(&m_pND); }
		if (m_IDXs) { cvFree(&m_IDXs); }
	}

	void Realloc(int dim, int* sizes, int type = SPARSE) {
		if (m_pSparse) { cvReleaseSparseMat(&m_pSparse); }
		if (m_pND) { cvReleaseMatND(&m_pND); }

		if (type == BYSIZE ) {
			int size = 0;
			int i;
			for (size = 1, i = 0; i < dim; ++i) {
				size *= sizes[i];
			}
			size *= sizeof(int);
			if (size > (2 << 20)) {
				/* if size > 1M */
				type = SPARSE;
			} else {
				type = ND;
			}
		} /* Define matrix type. */

		if (type == SPARSE) {
			m_pSparse = cvCreateSparseMat( dim, sizes, CV_32SC1 );
			m_Dim = dim;
		}
		if (type == ND ) {
			m_pND = cvCreateMatND( dim, sizes, CV_32SC1 );
			cvZero(m_pND);
			m_IDXs = (int*)cvAlloc(sizeof(int) * dim);
			m_Dim = dim;
		}
		m_Volume = 0;
		m_Max = 0;
	}
	void Save(const char* File) {
		if (m_pSparse) { cvSave(File, m_pSparse ); }
		if (m_pND) { cvSave(File, m_pND ); }
	}
	void Save(CvFileStorage* fs, const char* name) {
		if (m_pSparse) {
			cvWrite(fs, name, m_pSparse );
		} else if (m_pND) {
			cvWrite(fs, name, m_pND );
		}
	}
	void Load(const char* File) {
		CvFileStorage* fs = cvOpenFileStorage( File, NULL, CV_STORAGE_READ );
		if (fs) {
			void* ptr;
			if (m_pSparse) { cvReleaseSparseMat(&m_pSparse); }
			if (m_pND) { cvReleaseMatND(&m_pND); }
			m_Volume = 0;
			m_Max = 0;
			ptr = cvLoad(File);
			if (ptr && CV_IS_MATND_HDR(ptr)) { m_pND = (CvMatND*)ptr; }
			if (ptr && CV_IS_SPARSE_MAT_HDR(ptr)) { m_pSparse = (CvSparseMat*)ptr; }
			cvReleaseFileStorage(&fs);
		}
		AfterLoad();
	} /* Load. */

	void Load(CvFileStorage* fs, CvFileNode* node, const char* name) {
		CvFileNode* n = cvGetFileNodeByName(fs, node, name);
		void* ptr = n ? cvRead(fs, n) : NULL;
		if (ptr) {
			if (m_pSparse) { cvReleaseSparseMat(&m_pSparse); }
			if (m_pND) { cvReleaseMatND(&m_pND); }
			m_Volume = 0;
			m_Max = 0;
			if (CV_IS_MATND_HDR(ptr)) { m_pND = (CvMatND*)ptr; }
			if (CV_IS_SPARSE_MAT_HDR(ptr)) { m_pSparse = (CvSparseMat*)ptr; }
		} else {
			printf("WARNING!!! Can't load %s matrix\n", name);
		}
		AfterLoad();
	} /* Load. */

	void AfterLoad() {
		m_Volume = 0;
		m_Max = 0;
		if (m_pSparse) {
			/* Calculate Volume of loaded hist: */
			CvSparseMatIterator mat_iterator;
			CvSparseNode* node = cvInitSparseMatIterator( m_pSparse, &mat_iterator );

			for ( ; node != 0; node = cvGetNextSparseNode( &mat_iterator )) {
				int val = *(int*)CV_NODE_VAL( m_pSparse, node ); /* get value of the element
                                                                (assume that the type is CV_32SC1) */
				m_Volume += val;
				if (m_Max < val) { m_Max = val; }
			}
		} /* Calculate Volume of loaded hist. */

		if (m_pND) {
			/* Calculate Volume of loaded hist: */
			CvMat   mat;
			double  max_val;
			double  vol;
			cvGetMat( m_pND, &mat, NULL, 1 );

			vol = cvSum(&mat).val[0];
			m_Volume = cvRound(vol);
			cvMinMaxLoc( &mat, NULL, &max_val);
			m_Max = cvRound(max_val);
			/* MUST BE WRITTEN LATER */
		} /* Calculate Volume of loaded hist. */
	} /* AfterLoad. */

	int* GetPtr(int* indx) {
		if (m_pSparse) { return (int*)cvPtrND( m_pSparse, indx, NULL, 1, NULL); }
		if (m_pND) { return  (int*)cvPtrND( m_pND, indx, NULL, 1, NULL); }
		return NULL;
	} /* GetPtr. */

	int GetVal(int* indx) {
		int* p = GetPtr(indx);
		if (p) { return p[0]; }
		return -1;
	} /* GetVal. */

	int Add(int* indx, int val) {
		int  NewVal;
		int* pVal = GetPtr(indx);
		if (pVal == NULL) { return -1; }
		pVal[0] += val;
		NewVal = pVal[0];
		m_Volume += val;
		if (m_Max < NewVal) { m_Max = NewVal; }
		return NewVal;
	} /* Add. */

	void Add(DefMat* pMatAdd) {
		int*    pIDXS = NULL;
		int     Val = 0;
		for (Val = pMatAdd->GetNext(&pIDXS, 1 ); pIDXS; Val = pMatAdd->GetNext(&pIDXS, 0 )) {
			Add(pIDXS, Val);
		}
	} /* Add. */

	int SetMax(int* indx, int val) {
		int  NewVal;
		int* pVal = GetPtr(indx);
		if (pVal == NULL) { return -1; }
		if (val > pVal[0]) {
			m_Volume += val - pVal[0];
			pVal[0] = val;
		}
		NewVal = pVal[0];
		if (m_Max < NewVal) { m_Max = NewVal; }
		return NewVal;
	} /* Add. */

	int GetNext(int** pIDXS, int init = 0) {
		int     Val = 0;
		pIDXS[0] = NULL;
		if (m_pSparse) {
			m_pSparseNode = (init || m_pSparseNode == NULL) ?
							cvInitSparseMatIterator( m_pSparse, &m_SparseIterator ) :
							cvGetNextSparseNode( &m_SparseIterator );

			if (m_pSparseNode) {
				int* pVal = (int*)CV_NODE_VAL( m_pSparse, m_pSparseNode );
				if (pVal) { Val = pVal[0]; }
				pIDXS[0] = CV_NODE_IDX( m_pSparse, m_pSparseNode );
			}
		}/* Sparse matrix. */

		if (m_pND) {
			int i;
			if (init) {
				for (i = 0; i < m_Dim; ++i) {
					m_IDXs[i] = cvGetDimSize( m_pND, i ) - 1;
				}
				pIDXS[0] = m_IDXs;
				Val = GetVal(m_IDXs);
			} else {
				for (i = 0; i < m_Dim; ++i) {
					if ((m_IDXs[i]--) > 0) {
						break;
					}
					m_IDXs[i] = cvGetDimSize( m_pND, i ) - 1;
				}
				if (i == m_Dim) {
					pIDXS[0] = NULL;
				} else {
					pIDXS[0] = m_IDXs;
					Val = GetVal(m_IDXs);
				}

			} /* Get next ND. */

		} /* Sparse matrix. */

		return Val;

	}; /* GetNext. */
};

#define FV_NUM 10
#define FV_SIZE 10
typedef struct DefTrackFG {
	CvBlob                  blob;
	//    CvBlobTrackFVGen*       pFVGen;
	int                     LastFrame;
	float                   state;
	DefMat*                 pHist;
} DefTrackFG;
class CvBlobTrackAnalysisHist : public CvBlobTrackAnalysis {
	/*---------------- Internal functions: --------------------*/
private:
	int                 m_BinNumParam;
	int                 m_SmoothRadius;
	const char*         m_SmoothKernel;
	float               m_AbnormalThreshold;
	int                 m_TrackNum;
	int                 m_Frame;
	int                 m_BinNum;
	char                m_DataFileName[1024];
	int                 m_Dim;
	int*                m_Sizes;
	DefMat              m_HistMat;
	int                 m_HistVolumeSaved;
	int*                m_pFVi;
	int*                m_pFViVar;
	int*                m_pFViVarRes;
	CvBlobSeq           m_TrackFGList;
	//CvBlobTrackFVGen*   (*m_CreateFVGen)();
	CvBlobTrackFVGen*   m_pFVGen;
	void SaveHist() {
		if (m_DataFileName[0]) {
			m_HistMat.Save(m_DataFileName);
			m_HistVolumeSaved = m_HistMat.m_Volume;
		}
	};
	void LoadHist() {
		if (m_DataFileName[0]) { m_HistMat.Load(m_DataFileName); }
		m_HistVolumeSaved = m_HistMat.m_Volume;
	}
	void AllocData() {
		/* AllocData: */
		m_pFVi = (int*)cvAlloc(sizeof(int) * m_Dim);
		m_pFViVar = (int*)cvAlloc(sizeof(int) * m_Dim);
		m_pFViVarRes = (int*)cvAlloc(sizeof(int) * m_Dim);
		m_Sizes = (int*)cvAlloc(sizeof(int) * m_Dim);

		{
			/* Create init sparce matrix: */
			int     i;
			for (i = 0; i < m_Dim; ++i) { m_Sizes[i] = m_BinNum; }
			m_HistMat.Realloc(m_Dim, m_Sizes, SPARSE);
			m_HistVolumeSaved = 0;
		} /* Create init sparce matrix. */
	} /* AllocData. */

	void FreeData() {
		/* FreeData. */
		int i;
		for (i = m_TrackFGList.GetBlobNum(); i > 0; --i) {
			//DefTrackFG* pF = (DefTrackFG*)m_TrackFGList.GetBlob(i-1);
			//            pF->pFVGen->Release();
			m_TrackFGList.DelBlob(i - 1);
		}
		cvFree(&m_pFVi);
		cvFree(&m_pFViVar);
		cvFree(&m_pFViVarRes);
		cvFree(&m_Sizes);
	} /* FreeData. */

	virtual void ParamUpdate() {
		if (m_BinNum != m_BinNumParam) {
			FreeData();
			m_BinNum = m_BinNumParam;
			AllocData();
		}
	}
public:
	CvBlobTrackAnalysisHist(CvBlobTrackFVGen*   (*createFVGen)()): m_TrackFGList(sizeof(DefTrackFG)) {
		m_pFVGen = createFVGen();
		m_Dim = m_pFVGen->GetFVSize();
		m_Frame = 0;
		m_pFVi = 0;
		m_TrackNum = 0;
		m_BinNum = 32;
		m_DataFileName[0] = 0;

		m_AbnormalThreshold = 0.02f;
		AddParam("AbnormalThreshold", &m_AbnormalThreshold);
		CommentParam("AbnormalThreshold", "If trajectory histogram value is lesst then <AbnormalThreshold*DataBaseTrackNum> then trajectory is abnormal");

		m_SmoothRadius = 1;
		AddParam("SmoothRadius", &m_SmoothRadius);
		CommentParam("AbnormalThreshold", "Radius (in bins) for histogram smoothing");

		m_SmoothKernel = "L";
		AddParam("SmoothKernel", &m_SmoothKernel);
		CommentParam("SmoothKernel", "L - Linear, G - Gaussian");


		m_BinNumParam = m_BinNum;
		AddParam("BinNum", &m_BinNumParam);
		CommentParam("BinNum", "Number of bin for each dimention of feature vector");

		AllocData();
		SetModuleName("Hist");

	} /* Constructor. */

	~CvBlobTrackAnalysisHist() {
		SaveHist();
		FreeData();
		m_pFVGen->Release();
	} /* Destructor. */

	/*----------------- Interface: --------------------*/
	virtual void    AddBlob(CvBlob* pBlob) {
		DefTrackFG* pF = (DefTrackFG*)m_TrackFGList.GetBlobByID(CV_BLOB_ID(pBlob));
		if (pF == NULL) {
			/* create new filter */
			DefTrackFG F;
			F.state = 0;
			F.blob = pBlob[0];
			F.LastFrame = m_Frame;
			//            F.pFVGen = m_CreateFVGen();
			F.pHist = new DefMat(m_Dim, m_Sizes, SPARSE);
			m_TrackFGList.AddBlob((CvBlob*)&F);
			pF = (DefTrackFG*)m_TrackFGList.GetBlobByID(CV_BLOB_ID(pBlob));
		}

		assert(pF);
		pF->blob = pBlob[0];
		pF->LastFrame = m_Frame;
		m_pFVGen->AddBlob(pBlob);
	};
	virtual void Process(IplImage* pImg, IplImage* pFG) {
		int i;
		m_pFVGen->Process(pImg, pFG);
		int SK = m_SmoothKernel[0];

		for (i = 0; i < m_pFVGen->GetFVNum(); ++i) {
			int         BlobID = 0;
			float*      pFV = m_pFVGen->GetFV(i, &BlobID);
			float*      pFVMax = m_pFVGen->GetFVMax();
			float*      pFVMin = m_pFVGen->GetFVMin();
			DefTrackFG* pF = (DefTrackFG*)m_TrackFGList.GetBlobByID(BlobID);
			int         HistVal = 1;

			if (pFV == NULL) { break; }

			pF->LastFrame = m_Frame;

			{
				/* Binarize FV: */
				int         j;
				for (j = 0; j < m_Dim; ++j) {
					int     index;
					float   f0 = pFVMin ? pFVMin[j] : 0;
					float   f1 = pFVMax ? pFVMax[j] : 1;
					assert(f1 > f0);
					index = cvRound((m_BinNum - 1) * (pFV[j] - f0) / (f1 - f0));
					if (index < 0) { index = 0; }
					if (index >= m_BinNum) { index = m_BinNum - 1; }
					m_pFVi[j] = index;
				}
			}

			HistVal = m_HistMat.GetVal(m_pFVi);/* get bin value*/
			pF->state = 0;
			{
				/* Calculate state: */
				float   T = m_HistMat.m_Max * m_AbnormalThreshold; /* calc threshold */

				if (m_TrackNum > 0) { T = 256.0f * m_TrackNum * m_AbnormalThreshold; }
				if (T > 0) {
					pF->state = (T - HistVal) / (T * 0.2f) + 0.5f;
				}
				if (pF->state < 0) { pF->state = 0; }
				if (pF->state > 1) { pF->state = 1; }
			}

			{
				/* If it is a new FV then add it to trajectory histogram: */
				int i, flag = 1;
				int r = m_SmoothRadius;

				//                    printf("BLob %3d NEW FV [", CV_BLOB_ID(pF));
				//                    for(i=0;i<m_Dim;++i) printf("%d,", m_pFVi[i]);
				//                    printf("]");

				for (i = 0; i < m_Dim; ++i) {
					m_pFViVar[i] = -r;
				}

				while (flag) {
					float   dist = 0;
					int     HistAdd = 0;
					int     i;
					int     good = 1;
					for (i = 0; i < m_Dim; ++i) {
						m_pFViVarRes[i] = m_pFVi[i] + m_pFViVar[i];
						if (m_pFViVarRes[i] < 0) { good = 0; }
						if (m_pFViVarRes[i] >= m_BinNum) { good = 0; }
						dist += m_pFViVar[i] * m_pFViVar[i];
					}/* Calculate next dimension. */

					if (SK == 'G' || SK == 'g') {
						double dist2 = dist / (r * r);
						HistAdd = cvRound(256 * exp(-dist2)); /* Hist Add for (dist=1) = 25.6*/
					} else if (SK == 'L' || SK == 'l') {
						dist = (float)(sqrt(dist) / (r + 1));
						HistAdd = cvRound(256 * (1 - dist));
					} else {
						HistAdd = 255; /* Flat smoothing. */
					}

					if (good && HistAdd > 0) {
						/* Update histogram: */
						assert(pF->pHist);
						pF->pHist->SetMax(m_pFViVarRes, HistAdd);
					}   /* Update histogram. */

					for (i = 0; i < m_Dim; ++i) {
						/* Next config: */
						if ((m_pFViVar[i]++) < r) {
							break;
						}
						m_pFViVar[i] = -r;
					}   /* Increase next dimension variable. */
					if (i == m_Dim) { break; }
				}   /* Next variation. */
			} /* If new FV. */
		} /* Next FV. */

		{
			/* Check all blobs on list: */
			int i;
			for (i = m_TrackFGList.GetBlobNum(); i > 0; --i) {
				/* Add histogram and delete blob from list: */
				DefTrackFG* pF = (DefTrackFG*)m_TrackFGList.GetBlob(i - 1);
				if (pF->LastFrame + 3 < m_Frame && pF->pHist) {
					m_HistMat.Add(pF->pHist);
					delete pF->pHist;
					m_TrackNum++;
					m_TrackFGList.DelBlob(i - 1);
				}
			}/* next blob */
		}

		m_Frame++;

		if (m_Wnd) {
			/* Debug output: */
			int*        idxs = NULL;
			int         Val = 0;
			IplImage*   pI = cvCloneImage(pImg);

			cvZero(pI);

			for (Val = m_HistMat.GetNext(&idxs, 1); idxs; Val = m_HistMat.GetNext(&idxs, 0)) {
				/* Draw all elements: */
				float   vf;
				int     x, y;

				if (!idxs) { break; }
				if (Val == 0) { continue; }

				vf = (float)Val / (m_HistMat.m_Max ? m_HistMat.m_Max : 1);
				x = cvRound((float)(pI->width - 1) * (float)idxs[0] / (float)m_BinNum);
				y = cvRound((float)(pI->height - 1) * (float)idxs[1] / (float)m_BinNum);

				cvCircle(pI, cvPoint(x, y), cvRound(vf * pI->height / (m_BinNum * 2)), CV_RGB(255, 0, 0), CV_FILLED);
				if (m_Dim > 3) {
					int dx = -2 * (idxs[2] - m_BinNum / 2);
					int dy = -2 * (idxs[3] - m_BinNum / 2);
					cvLine(pI, cvPoint(x, y), cvPoint(x + dx, y + dy), CV_RGB(0, cvRound(vf * 255), 1));
				}
				if ( m_Dim == 4 &&
						m_pFVGen->GetFVMax()[0] == m_pFVGen->GetFVMax()[2] &&
						m_pFVGen->GetFVMax()[1] == m_pFVGen->GetFVMax()[3]) {
					int x = cvRound((float)(pI->width - 1) * (float)idxs[2] / (float)m_BinNum);
					int y = cvRound((float)(pI->height - 1) * (float)idxs[3] / (float)m_BinNum);
					cvCircle(pI, cvPoint(x, y), cvRound(vf * pI->height / (m_BinNum * 2)), CV_RGB(0, 0, 255), CV_FILLED);
				}
			} /* Draw all elements. */

			for (i = m_TrackFGList.GetBlobNum(); i > 0; --i) {
				DefTrackFG* pF = (DefTrackFG*)m_TrackFGList.GetBlob(i - 1);
				DefMat* pHist = pF ? pF->pHist : NULL;

				if (pHist == NULL) { continue; }

				for (Val = pHist->GetNext(&idxs, 1); idxs; Val = pHist->GetNext(&idxs, 0)) {
					/* Draw all elements: */
					float   vf;
					int     x, y;

					if (!idxs) { break; }
					if (Val == 0) { continue; }

					vf = (float)Val / (pHist->m_Max ? pHist->m_Max : 1);
					x = cvRound((float)(pI->width - 1) * (float)idxs[0] / (float)m_BinNum);
					y = cvRound((float)(pI->height - 1) * (float)idxs[1] / (float)m_BinNum);

					cvCircle(pI, cvPoint(x, y), cvRound(2 * vf), CV_RGB(0, 0, cvRound(255 * vf)), CV_FILLED);
					if (m_Dim > 3) {
						int dx = -2 * (idxs[2] - m_BinNum / 2);
						int dy = -2 * (idxs[3] - m_BinNum / 2);
						cvLine(pI, cvPoint(x, y), cvPoint(x + dx, y + dy), CV_RGB(0, 0, 255));
					}
					if ( m_Dim == 4 &&
							m_pFVGen->GetFVMax()[0] == m_pFVGen->GetFVMax()[2] &&
							m_pFVGen->GetFVMax()[1] == m_pFVGen->GetFVMax()[3]) {
						/* if SS feature vector */
						int x = cvRound((float)(pI->width - 1) * (float)idxs[2] / (float)m_BinNum);
						int y = cvRound((float)(pI->height - 1) * (float)idxs[3] / (float)m_BinNum);
						cvCircle(pI, cvPoint(x, y), cvRound(vf * pI->height / (m_BinNum * 2)), CV_RGB(0, 0, 255), CV_FILLED);
					}
				} /* Draw all elements. */
			} /* Next track. */

			//cvNamedWindow("Hist",0);
			//cvShowImage("Hist", pI);
			cvReleaseImage(&pI);
		}
	};

	float GetState(int BlobID) {
		DefTrackFG* pF = (DefTrackFG*)m_TrackFGList.GetBlobByID(BlobID);
		return pF ? pF->state : 0.0f;
	};

	/* Return 0 if trajectory is normal;
	   rreturn >0 if trajectory abnormal. */
	virtual const char*   GetStateDesc(int BlobID) {
		if (GetState(BlobID) > 0.5) { return "abnormal"; }
		return NULL;
	}

	virtual void    SetFileName(const char* DataBaseName) {
		if (m_HistMat.m_Volume != m_HistVolumeSaved) { SaveHist(); }
		m_DataFileName[0] = 0;

		if (DataBaseName) {
			strncpy(m_DataFileName, DataBaseName, 1000);
			strcat(m_DataFileName, ".yml");
		}
		LoadHist();
	};

	virtual void SaveState(CvFileStorage* fs) {
		int b, bN = m_TrackFGList.GetBlobNum();
		cvWriteInt(fs, "BlobNum", bN);
		cvStartWriteStruct(fs, "BlobList", CV_NODE_SEQ);

		for (b = 0; b < bN; ++b) {
			DefTrackFG* pF = (DefTrackFG*)m_TrackFGList.GetBlob(b);
			cvStartWriteStruct(fs, NULL, CV_NODE_MAP);
			cvWriteStruct(fs, "Blob", &(pF->blob), "ffffi");
			cvWriteInt(fs, "LastFrame", pF->LastFrame);
			cvWriteReal(fs, "State", pF->state);
			pF->pHist->Save(fs, "Hist");
			cvEndWriteStruct(fs);
		}
		cvEndWriteStruct(fs);
		m_HistMat.Save(fs, "Hist");
	};

	virtual void LoadState(CvFileStorage* fs, CvFileNode* node) {
		CvFileNode* pBLN = cvGetFileNodeByName(fs, node, "BlobList");

		if (pBLN && CV_NODE_IS_SEQ(pBLN->tag)) {
			int b, bN = pBLN->data.seq->total;
			for (b = 0; b < bN; ++b) {
				DefTrackFG* pF = NULL;
				CvBlob      Blob;
				CvFileNode* pBN = (CvFileNode*)cvGetSeqElem(pBLN->data.seq, b);

				assert(pBN);
				cvReadStructByName(fs, pBN, "Blob", &Blob, "ffffi");
				AddBlob(&Blob);
				pF = (DefTrackFG*)m_TrackFGList.GetBlobByID(Blob.ID);
				if (pF == NULL) { continue; }
				assert(pF);
				pF->state = (float)cvReadIntByName(fs, pBN, "State", cvRound(pF->state));
				assert(pF->pHist);
				pF->pHist->Load(fs, pBN, "Hist");
			}
		}

		m_HistMat.Load(fs, node, "Hist");
	}; /* LoadState */


	virtual void    Release() { delete this; };

};



CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisHistP()
{return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisHist(cvCreateFVGenP);}

CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisHistPV()
{return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisHist(cvCreateFVGenPV);}

CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisHistPVS()
{return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisHist(cvCreateFVGenPVS);}

CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisHistSS()
{return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisHist(cvCreateFVGenSS);}

typedef struct DefTrackSVM {
	CvBlob                  blob;
	//    CvBlobTrackFVGen*       pFVGen;
	int                     LastFrame;
	float                   state;
	CvBlob                  BlobLast;
	CvSeq*                  pFVSeq;
	CvMemStorage*           pMem;
} DefTrackSVM;

class CvBlobTrackAnalysisSVM : public CvBlobTrackAnalysis {
	/*---------------- Internal functions: --------------------*/
private:
	CvMemStorage*       m_pMem;
	int                 m_TrackNum;
	int                 m_Frame;
	char                m_DataFileName[1024];
	int                 m_Dim;
	float*              m_pFV;
	//CvStatModel*        m_pStatModel;
	void*               m_pStatModel;
	CvBlobSeq           m_Tracks;
	CvMat*              m_pTrainData;
	int                 m_LastTrainDataSize;
	//    CvBlobTrackFVGen*   (*m_CreateFVGen)();
	CvBlobTrackFVGen*   m_pFVGen;
	float               m_NU;
	float               m_RBFWidth;
	IplImage*           m_pStatImg; /* for debug purpose */
	CvSize              m_ImgSize;
	void RetrainStatModel() {
		///////// !!!!! TODO !!!!! Repair /////////////
#if 0
		float               nu = 0;
		CvSVMModelParams    SVMParams = {0};
		CvStatModel*        pM = NULL;


		memset(&SVMParams, 0, sizeof(SVMParams));
		SVMParams.svm_type = CV_SVM_ONE_CLASS;
		SVMParams.kernel_type = CV_SVM_RBF;
		SVMParams.gamma = 2.0 / (m_RBFWidth * m_RBFWidth);
		SVMParams.nu = m_NU;
		SVMParams.degree = 3;
		SVMParams.criteria = cvTermCriteria(CV_TERMCRIT_EPS, 100, 1e-3 );
		SVMParams.C = 1;
		SVMParams.p = 0.1;


		if (m_pTrainData == NULL) { return; }
		{
			int64       TickCount = cvGetTickCount();
			printf("Frame: %d\n           Retrain SVM\nData Size = %d\n", m_Frame, m_pTrainData->rows);
			pM = cvTrainSVM( m_pTrainData, CV_ROW_SAMPLE, NULL, (CvStatModelParams*)&SVMParams, NULL, NULL);
			TickCount = cvGetTickCount() - TickCount ;
			printf("SV Count = %d\n", ((CvSVMModel*)pM)->sv_total);
			printf("Processing Time = %.1f(ms)\n", TickCount / (1000 * cvGetTickFrequency()));

		}
		if (pM == NULL) { return; }
		if (m_pStatModel) { cvReleaseStatModel(&m_pStatModel); }
		m_pStatModel = pM;

		if (m_pTrainData && m_Wnd) {
			float       MaxVal = 0;
			IplImage*   pW = cvCreateImage(m_ImgSize, IPL_DEPTH_32F, 1);
			IplImage*   pI = cvCreateImage(m_ImgSize, IPL_DEPTH_8U, 1);
			float*      pFVVar = m_pFVGen->GetFVVar();
			int     i;
			cvZero(pW);

			for (i = 0; i < m_pTrainData->rows; ++i) {
				/* Draw all elements: */
				float*          pFV = (float*)(m_pTrainData->data.ptr + m_pTrainData->step * i);
				int             x = cvRound(pFV[0] * pFVVar[0]);
				int             y = cvRound(pFV[1] * pFVVar[1]);
				float           r;

				if (x < 0) { x = 0; }
				if (x >= pW->width) { x = pW->width - 1; }
				if (y < 0) { y = 0; }
				if (y >= pW->height) { y = pW->height - 1; }

				r = ((float*)(pW->imageData + y * pW->widthStep))[x];
				r++;
				((float*)(pW->imageData + y * pW->widthStep))[x] = r;

				if (r > MaxVal) { MaxVal = r; }
			} /* Next point. */

			if (MaxVal > 0) { cvConvertScale(pW, pI, 255 / MaxVal, 0); }
			cvNamedWindow("SVMData", 0);
			cvShowImage("SVMData", pI);
			cvSaveImage("SVMData.bmp", pI);
			cvReleaseImage(&pW);
			cvReleaseImage(&pI);
		} /* Prepare for debug. */

		if (m_pStatModel && m_Wnd && m_Dim == 2) {
			float*      pFVVar = m_pFVGen->GetFVVar();
			int x, y;
			if (m_pStatImg == NULL) {
				m_pStatImg = cvCreateImage(m_ImgSize, IPL_DEPTH_8U, 1);
			}
			cvZero(m_pStatImg);

			for (y = 0; y < m_pStatImg->height; y += 1) for (x = 0; x < m_pStatImg->width; x += 1) {
					/* Draw all elements: */
					float           res;
					uchar*  pData = (uchar*)m_pStatImg->imageData + x + y * m_pStatImg->widthStep;
					CvMat           FVmat;
					float           xy[2] = {x / pFVVar[0], y / pFVVar[1]};
					cvInitMatHeader( &FVmat, 1, 2, CV_32F, xy );
					res = cvStatModelPredict( m_pStatModel, &FVmat, NULL );
					pData[0] = ((res > 0.5) ? 255 : 0);
				} /* Next point. */

			cvNamedWindow("SVMMask", 0);
			cvShowImage("SVMMask", m_pStatImg);
			cvSaveImage("SVMMask.bmp", m_pStatImg);
		} /* Prepare for debug. */
#endif
	};
	void SaveStatModel() {
		if (m_DataFileName[0]) {
			if (m_pTrainData) { cvSave(m_DataFileName, m_pTrainData); }
		}
	};
	void LoadStatModel() {
		if (m_DataFileName[0]) {
			CvMat* pTrainData = (CvMat*)cvLoad(m_DataFileName);
			if (CV_IS_MAT(pTrainData) && pTrainData->width == m_Dim) {
				if (m_pTrainData) { cvReleaseMat(&m_pTrainData); }
				m_pTrainData = pTrainData;
				RetrainStatModel();
			}
		}
	}
public:
	CvBlobTrackAnalysisSVM(CvBlobTrackFVGen*   (*createFVGen)()): m_Tracks(sizeof(DefTrackSVM)) {
		m_pFVGen = createFVGen();
		m_Dim = m_pFVGen->GetFVSize();
		m_pFV = (float*)cvAlloc(sizeof(float) * m_Dim);
		m_Frame = 0;
		m_TrackNum = 0;
		m_pTrainData = NULL;
		m_pStatModel = NULL;
		m_DataFileName[0] = 0;
		m_pStatImg = NULL;
		m_LastTrainDataSize = 0;

		m_NU = 0.2f;
		AddParam("Nu", &m_NU);
		CommentParam("Nu", "Parameters that tunes SVM border elastic");

		m_RBFWidth = 1;
		AddParam("RBFWidth", &m_RBFWidth);
		CommentParam("RBFWidth", "Parameters that tunes RBF kernel function width.");

		SetModuleName("SVM");

	} /* Constructor. */

	~CvBlobTrackAnalysisSVM() {
		int i;
		SaveStatModel();
		for (i = m_Tracks.GetBlobNum(); i > 0; --i) {
			DefTrackSVM* pF = (DefTrackSVM*)m_Tracks.GetBlob(i - 1);
			if (pF->pMem) { cvReleaseMemStorage(&pF->pMem); }
			//pF->pFVGen->Release();
		}
		if (m_pStatImg) { cvReleaseImage(&m_pStatImg); }
		cvFree(&m_pFV);
	} /* Destructor. */

	/*----------------- Interface: --------------------*/
	virtual void    AddBlob(CvBlob* pBlob) {
		DefTrackSVM* pF = (DefTrackSVM*)m_Tracks.GetBlobByID(CV_BLOB_ID(pBlob));

		m_pFVGen->AddBlob(pBlob);

		if (pF == NULL) {
			/* Create new record: */
			DefTrackSVM F;
			F.state = 0;
			F.blob = pBlob[0];
			F.LastFrame = m_Frame;
			//F.pFVGen = m_CreateFVGen();
			F.pMem = cvCreateMemStorage();
			F.pFVSeq = cvCreateSeq(0, sizeof(CvSeq), sizeof(float) * m_Dim, F.pMem);

			F.BlobLast.x = -1;
			F.BlobLast.y = -1;
			F.BlobLast.w = -1;
			F.BlobLast.h = -1;
			m_Tracks.AddBlob((CvBlob*)&F);
			pF = (DefTrackSVM*)m_Tracks.GetBlobByID(CV_BLOB_ID(pBlob));
		}

		assert(pF);
		pF->blob = pBlob[0];
		pF->LastFrame = m_Frame;
	};

	virtual void Process(IplImage* pImg, IplImage* pFG) {
		int     i;
		float*  pFVVar = m_pFVGen->GetFVVar();

		m_pFVGen->Process(pImg, pFG);
		m_ImgSize = cvSize(pImg->width, pImg->height);

		for (i = m_pFVGen->GetFVNum(); i > 0; --i) {
			int             BlobID = 0;
			float*          pFV = m_pFVGen->GetFV(i, &BlobID);
			DefTrackSVM*    pF = (DefTrackSVM*)m_Tracks.GetBlobByID(BlobID);

			if (pF && pFV) {
				/* Process: */
				float   dx, dy;
				CvMat   FVmat;

				pF->state = 0;

				if (m_pStatModel) {
					int j;
					for (j = 0; j < m_Dim; ++j) {
						m_pFV[j] = pFV[j] / pFVVar[j];
					}

					cvInitMatHeader( &FVmat, 1, m_Dim, CV_32F, m_pFV );
					//pF->state = cvStatModelPredict( m_pStatModel, &FVmat, NULL )<0.5;
					pF->state = 1.f;
				}

				dx = (pF->blob.x - pF->BlobLast.x);
				dy = (pF->blob.y - pF->BlobLast.y);

				if (pF->BlobLast.x < 0 || (dx * dx + dy * dy) >= 2 * 2) {
					/* Add feature vector to train data base: */
					pF->BlobLast = pF->blob;
					cvSeqPush(pF->pFVSeq, pFV);
				}
			} /* Process one blob. */
		} /* Next FV. */

		for (i = m_Tracks.GetBlobNum(); i > 0; --i) {
			/* Check each blob record: */
			DefTrackSVM* pF = (DefTrackSVM*)m_Tracks.GetBlob(i - 1);

			if (pF->LastFrame + 3 < m_Frame ) {
				/* Retrain stat model and delete blob filter: */
				int                 mult = 1 + m_Dim;
				int                 old_height = m_pTrainData ? m_pTrainData->height : 0;
				int                 height = old_height + pF->pFVSeq->total * mult;
				CvMat*              pTrainData = cvCreateMat(height, m_Dim, CV_32F);
				int                 j;
				if (m_pTrainData && pTrainData) {
					/* Create new train data matrix: */
					int h = pTrainData->height;
					pTrainData->height = MIN(pTrainData->height, m_pTrainData->height);
					cvCopy(m_pTrainData, pTrainData);
					pTrainData->height = h;
				}

				for (j = 0; j < pF->pFVSeq->total; ++j) {
					/* Copy new data to train data: */
					float*  pFVVar = m_pFVGen->GetFVVar();
					float*  pFV = (float*)cvGetSeqElem(pF->pFVSeq, j);
					int     k;

					for (k = 0; k < mult; ++k) {
						int t;
						float*  pTD = (float*)CV_MAT_ELEM_PTR( pTrainData[0], old_height + j * mult + k, 0);
						memcpy(pTD, pFV, sizeof(float)*m_Dim);

						if (pFVVar)for (t = 0; t < m_Dim; ++t) {
								/* Scale FV: */
								pTD[t] /= pFVVar[t];
							}

						if (k > 0) {
							/* Variate: */
							for (t = 0; t < m_Dim; ++t) {
								pTD[t] += m_RBFWidth * 0.5f * (1 - 2.0f * rand() / (float)RAND_MAX);
							}
						}
					}
				} /* Next new datum. */

				if (m_pTrainData) { cvReleaseMat(&m_pTrainData); }
				m_pTrainData = pTrainData;

				/* delete track record */
				cvReleaseMemStorage(&pF->pMem);
				m_TrackNum++;
				m_Tracks.DelBlob(i - 1);

			} /* End delete. */
		} /* Next track. */

		/* Retrain data each 1 minute if new data exist: */
		if (m_Frame % (25 * 60) == 0 && m_pTrainData && m_pTrainData->rows > m_LastTrainDataSize) {
			RetrainStatModel();
		}

		m_Frame++;

		if (m_Wnd && m_Dim == 2) {
			/* Debug output: */
			int         x, y;
			IplImage*   pI = cvCloneImage(pImg);

			if (m_pStatModel && m_pStatImg)

				for (y = 0; y < pI->height; y += 2) {
					uchar*  pStatData = (uchar*)m_pStatImg->imageData + y * m_pStatImg->widthStep;
					uchar*  pData = (uchar*)pI->imageData + y * pI->widthStep;

					for (x = 0; x < pI->width; x += 2) {
						/* Draw all elements: */
						int d = pStatData[x];
						d = (d << 8) | (d ^ 0xff);
						*(ushort*)(pData + x * 3) = (ushort)d;
					}
				} /* Next line. */

			//cvNamedWindow("SVMMap",0);
			//cvShowImage("SVMMap", pI);
			cvReleaseImage(&pI);
		} /* Debug output. */
	};
	float GetState(int BlobID) {
		DefTrackSVM* pF = (DefTrackSVM*)m_Tracks.GetBlobByID(BlobID);
		return pF ? pF->state : 0.0f;
	};

	/* Return 0 if trajectory is normal;
	   return >0 if trajectory abnormal. */
	virtual const char*   GetStateDesc(int BlobID) {
		if (GetState(BlobID) > 0.5) { return "abnormal"; }
		return NULL;
	}

	virtual void    SetFileName(char* DataBaseName) {
		if (m_pTrainData) { SaveStatModel(); }
		m_DataFileName[0] = 0;
		if (DataBaseName) {
			strncpy(m_DataFileName, DataBaseName, 1000);
			strcat(m_DataFileName, ".yml");
		}
		LoadStatModel();
	};


	virtual void    Release() { delete this; };

}; /* CvBlobTrackAnalysisSVM. */


CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisSVMP()
{return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisSVM(cvCreateFVGenP);}

CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisSVMPV()
{return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisSVM(cvCreateFVGenPV);}

CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisSVMPVS()
{return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisSVM(cvCreateFVGenPVS);}

CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisSVMSS()
{return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisSVM(cvCreateFVGenSS);}
